#include <Servo.h> 

//////////////////////////////////////////////////////////////////////////
//                      Definitions and Constants                       //
//////////////////////////////////////////////////////////////////////////

// Number of Servos
#define NSERVOS 12

// PING Sensor source and destination pins
#define dstPIN 49
#define srcPIN 47

// Servo array
Servo FACE[NSERVOS];                  
// Servo pin array
byte FACE_PIN[NSERVOS] = {23,25,27,29,31,33,30,32,28,24,26,35};
// Counters
byte x,i,count = 0;
//Flags: enable, Mode (speed or position), PING Pin recently SET
byte EN,MD,SET = 0;

//////////////////////////////////////////////////////////////////////////
//                      FUNCTION PROTOTYPES                             //
//////////////////////////////////////////////////////////////////////////

// Ping Sensor Function
boolean inRange(unsigned int pingPin, unsigned long duration_max);
//////////////////////////////////////////////////////////////////////////
//                          Main Application                            //
//////////////////////////////////////////////////////////////////////////

void setup() {
        Serial.begin(115200);
        // Attach Servos to pins    
        for(i = 0; i<NSERVOS; i++)  {
          FACE[i].attach(FACE_PIN[i]);
        }
        // Init dst PING pin (output to iMX6)
        pinMode(dstPIN,OUTPUT);
}

void loop() {
  while(Serial.available()) {
    x=Serial.read();
    if (x == 255) {
	EN = 1;
	MD = 0;
	count = 0;
    }
    else if(x == 254) {
	EN = 1;
	MD = 1;
	count = 0;
    }
    else if(EN == 1 && MD == 1) {
      // if 0 Skip motor
        if(x > 0) {
	  FACE[count].write(x);
        }
	count++;
	if (count == NSERVOS)
	    EN = 0;
    }
   }
    // Check PING Sensor Status
    //print
    if (inRange(srcPIN,5800) == true) {
      digitalWrite(dstPIN,HIGH);
      SET = 1;
    }
    // Set PING output to 0, only if there is a state change
    else if (SET == 1) {
      digitalWrite(dstPIN,LOW);
      SET = 0;
    }
  
  delay(20);
}

boolean inRange(unsigned int pingPin, unsigned long duration_max) {
  
  long duration;
  
  // The PING))) is triggered by a HIGH pulse of 2 or more microseconds.
  // Give a short LOW pulse beforehand to ensure a clean HIGH pulse:
  pinMode(pingPin, OUTPUT);
  digitalWrite(pingPin, LOW);
  delayMicroseconds(2);
  digitalWrite(pingPin, HIGH);
  delayMicroseconds(5);
  digitalWrite(pingPin, LOW);

  // The same pin is used to read the signal from the PING))): a HIGH
  // pulse whose duration is the time (in microseconds) from the sending
  // of the ping to the reception of its echo off of an object.
  pinMode(pingPin, INPUT);
  duration = pulseIn(pingPin, HIGH);
  
  if (duration < duration_max) 
    return true;
    
  return false;
}
